/*
 * net/sched/sch_empath.c	Emulator of a Network Path
 *
 * 		This program is free software; you can redistribute it and/or
 * 		modify it under the terms of the GNU General Public License
 * 		as published by the Free Software Foundation; either version
 * 		2 of the License.
 *
 * Authors:	Jaroslaw Sliwinski <jarek.sliwinski@gmail.com>
 *              
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/skbuff.h>
#include <linux/rtnetlink.h>

#include <net/netlink.h>
#include <net/pkt_sched.h>

#include "pkt_sched_empath.h"

#define VERSION "1.0"

/*	Emulation of a Network Path algorithm.
	====================================

	 This qdisc does not do classification that can be handled in
	 layering other disciplines.  It does not need to do bandwidth
	 control either since that can be handled by using token
	 bucket or other rate control.

	 The simulator is limited by the Linux timer resolution
	 and will create packet bursts on the HZ boundary (1ms).
*/

/*
#define DEBUG
*/

#ifdef print_debug
#undef print_debug
#endif

#ifdef DEBUG
#define print_debug pr_info("empath: "__VA_ARGS__)
#else
#define print_debug(...) {}
#endif

#define print_info(...) pr_info("empath: "__VA_ARGS__)

struct empath_dist_entry {
	__u32 x;                 /* argument of distribution */
	__u32 y_low;             /* low limit of distribution */
	__u32 y_high;            /* high limit of distribution */
	unsigned int next_table; /* index of next table for use */
	unsigned int search_low; /* index for lower search */
	unsigned int search_high;/* index for higher search */
};

struct empath_dist
{
	__u32 packet_loss;  
	int entries_count;  /* number of entries in distribution */
	int entries_parent;
	struct empath_dist_entry entries[];
};

struct empath_interarrival
{
	__u32 interarrival; /* interarrival period for this distribution */
	int dist_count;
	struct empath_dist **dists;
};


struct empath_sched_data {
	struct Qdisc *qdisc;
	struct qdisc_watchdog watchdog;
	/* delay randomization structues */
	int interarrival_count;             /* total number of interarrivals */
	struct empath_interarrival *interarrivals; /* interarrivals table */
	int dist_count;
	int dist_state;                     /* distribution state used for generation */
	psched_time_t arrival;              /* arrival moment of previous packet */
	psched_tdiff_t previous_delay;
	int previous_loss;
	psched_time_t scope_delay;
};

/* Time stamp that is put into socket buffer control block */
struct empath_skb_cb {
	psched_time_t	time_to_send;
};

static inline struct empath_skb_cb *empath_skb_cb(struct sk_buff *skb)
{
	BUILD_BUG_ON(sizeof(skb->cb) <
		sizeof(struct qdisc_skb_cb) + sizeof(struct empath_skb_cb));
	return (struct empath_skb_cb *)qdisc_skb_cb(skb)->data;
}


/* 
 * Searches distribution entry for given value.
 */
static inline int dist_search(struct empath_dist *dist, u32 r)
{
	int i;
	int count = 0;
	i = dist->entries_parent;
	while(count++ < 255)
	{
		/*print_debug("netem: dist_search %08x? %d %08x<->%08x %d<->%d\n",
			 r, 
			 i, dist->entries[i].y_low, dist->entries[i].y_high, 
			 dist->entries[i].search_low, dist->entries[i].search_high);*/
		if (i < 0)
		{
			print_info("error, negative distribution index\n");
			return 0;
		}
		if (r < dist->entries[i].y_low)
		{
			i = dist->entries[i].search_low;
			continue;
		} else if (r > dist->entries[i].y_high) {
			i = dist->entries[i].search_high;
			continue;
		} else {
			return i;
		}
	}
	print_info("error, more than 255 searches in dist_search, loop problem?\n");
	return i;
}

/* dist_generate - return a pseudo-randomly distributed value of useconds */
static inline psched_tdiff_t dist_generate(
	struct empath_sched_data *data,
	int *is_lost, psched_time_t *now)
{
	int i, k;
	psched_tdiff_t interarrival;
	struct empath_dist *dist;
	/* if we had huge gap in the interarrivals -> generate from largest */
	if (data->arrival + data->scope_delay < *now)
	{
		print_debug("line %d: time skip\n", __LINE__);
		data->arrival = *now - data->scope_delay;
	}
	/* progress the past up to present */
	k =  data->interarrival_count - 1;
	interarrival = data->interarrivals[k].interarrival;
	while (data->arrival < *now)
	{
		while (data->arrival + interarrival > *now && k > 0)
		{
			k--;
			interarrival = data->interarrivals[k].interarrival;
		}
		data->arrival += interarrival;
		dist = data->interarrivals[k].dists[data->dist_state];
		/* check if packet should be lost */
		if (net_random() < dist->packet_loss)
		{
			data->previous_loss = 1;
			data->dist_state = data->dist_count - 1; /* "lost" dist */
			continue;
		}
		data->previous_loss = 0;
		/* generate new delay */
		i = dist_search( dist, net_random());
		data->previous_delay = dist->entries[i].x;
		data->dist_state = dist->entries[i].next_table;
	}
	*is_lost = data->previous_loss;
	print_debug("dist_generate() delay=%lu is_lost=%d state=%d\n",
		data->previous_delay, data->previous_loss, data->dist_state);
	return data->previous_delay;
}

/* creates binary search structure over distribution - it is slow */

static void dist_prepare_search_internal(
	struct empath_dist_entry *entries, int parent,
	int bottom, int top);

static void dist_prepare_search(struct empath_dist *dist)
{
	int i;
	u32 value = 1 << 31;
	print_debug("dist_prepare_search() start entries_count=%d\n", 
		dist->entries_count);
	/* find location of median value */
	for (i = 0; i < dist->entries_count; i++)
	{
		print_debug("entry=%d value=%x low=%x high=%x\n", 
			i, value, dist->entries[i].y_low, dist->entries[i].y_high);
		if (dist->entries[i].y_low <= value &&
		    value <= dist->entries[i].y_high)
			break;
	}
	if ( ! (i < dist->entries_count) )
	{
		print_debug("dist_prepare_search() median not found\n");
		return;
	}
	print_debug("dist_prepare_search() median found=%d\n", i);
	dist->entries_parent = i;
	dist->entries[i].search_low = -1;
	dist->entries[i].search_high = -1;
	dist_prepare_search_internal(
		dist->entries, dist->entries_parent,
		0, dist->entries_count-1);
	/* and we are done! */
	print_debug("dist_prepare_search() end\n");
}

static void dist_prepare_search_internal(
	struct empath_dist_entry *entries, int parent,
	int bottom, int top)
{
	int i;
	u32 t;
	print_debug("dist_prepare_search_internal() start %d->[%d:%d]\n",
		 parent, bottom, top);
	/* find location with lower value */
	if (bottom < parent)
	{
		t = entries[bottom].y_low;
		t += (entries[parent-1].y_high - entries[bottom].y_low) >> 1;
		for (i = bottom; i < parent - 1; i++)
		{
			if (entries[i].y_low <= t && t <= entries[i].y_high)
				break;
		}
		print_debug("dist_prepare_search_internal() 'lower' parent->%d\n", i);
		entries[parent].search_low = i;
		entries[i].search_low = -1;
		entries[i].search_high = -1;
		dist_prepare_search_internal(
			entries, i,
			bottom, i-1);
		dist_prepare_search_internal(
			entries, i,
			i+1, parent-1);
	}
	/* find location with greater value */
	if (parent < top)
	{
		t = entries[parent+1].y_low;
		t += (entries[top].y_high - entries[parent+1].y_low) >> 1;
		for (i = parent+1; i < top; i++)
		{
			if (entries[i].y_low <= t && t <= entries[i].y_high)
				break;
		}
		print_debug("dist_prepare_search_internal() 'higher' parent->%d"
			 " %08x <= %08x <= %08x\n",
			 i, entries[i].y_low, t, entries[i].y_high);
		entries[parent].search_high = i;
		entries[i].search_low = -1;
		entries[i].search_high = -1;
		dist_prepare_search_internal(
			entries, i,
			parent+1, i-1);
		dist_prepare_search_internal(
			entries, i,
			i+1, top);
	}
	print_debug("dist_prepare_search_internal() end %d->[%d:%d]\n",
		 parent, bottom, top);
}

/* 
 *  Returns a sorted table of all unique arguments.
 */
static u32 *interarrival_get_unique_arguments(
	struct empath_interarrival *interarrival,
	int *count)
{
	u32 i, j, t;
	u32 *result, *result2;
	/* first aproximation of table size */
	t = 0;
	for (i = 0; i < interarrival->dist_count; i++)
		t += interarrival->dists[i]->entries_count;
	result = kmalloc(t*sizeof(u32), GFP_KERNEL);
	if (!result)
	{
		*count = 0;
		return NULL;
	}
	*count = t;
	/* copy all values */
	t = 0;
	for (i = 0; i < interarrival->dist_count; i++)
	{
		struct empath_dist *dist;
		dist = interarrival->dists[i];
		for (j = 0; j < dist->entries_count; j++)
		{
			result[t] = dist->entries[j].x;
			t++;
		}
	}
	print_debug("arguments before filtering\n");
	for (i = 0; i < *count; i++)
	{
		print_debug("i=%d x=%d\n", i, result[i]);
	}
	
	/* filter out repeating values */
	t = *count;
	for (i = 0; i < *count; i++)
	{
		for (j = i+1; j < t; j++)
		{
			if (result[i] == result[j])
			{
				result[j] = result[t-1];
				t--; j--;
			}
		}
	}
	*count = t;
	print_debug("arguments after filtering\n");
	for (i = 0; i < *count; i++)
	{
		print_debug("i=%d x=%d\n", i, result[i]);
	}
	/* prepare result table */
	result2 = kmalloc(*count * sizeof(u32), GFP_KERNEL);
	if (!result2)
	{
		*count = 0;
		kfree(result);
		return NULL;
	}
	for (i = 0; i < *count; i++)
	{
		result2[i] = result[i];
	}
	kfree(result);
	result = result2;
	/* sort result table */
	for (i = 0 ; i < *count; i++)
	{
		int best = i;
		for (j = i+1; j < *count; j++)
			if (result[j] < result[best])
				best = j;
		t = result[best];
		result[best] = result[i];
		result[i] = t;
	}
	return result;
}

/* 
   Creates a new interarrival structure for double the time.
   Returns negative integer when failed. It is slow.
*/

static int interarrival_power(
	struct empath_interarrival *source,
	struct empath_interarrival *destination)
{
	u32 *unique_arguments = NULL;
	int unique_arguments_count;
	u64 *values = NULL, *losses = NULL;
	int *jumps = NULL;
	int ret, i, j, k, m, t;
	u32 val;
	struct empath_dist *dist1, *dist2, *distN;
	u64 p1, p2, p3, p4, p5;
	struct empath_dist *dist;
	u64 sum, sum_low;
		
	print_debug("interarrival_power() start power for interarrival=%u\n",
		source->interarrival);
	/* unique arguments - they limit the number of states */
	unique_arguments = interarrival_get_unique_arguments(
		source, &unique_arguments_count);
	if (!unique_arguments)
	{
		print_info("failed to create unique_arguments\n");
		ret = -1;
		goto finish;
	}
	/* temporary strucutres for calculation */
	values = kmalloc(
		source->dist_count*unique_arguments_count*sizeof(u64), GFP_KERNEL);
	jumps = kmalloc(unique_arguments_count*sizeof(int), GFP_KERNEL);
	losses = kmalloc(source->dist_count*sizeof(u64), GFP_KERNEL);
	if (!values || !jumps || !losses)
	{
		print_info("failed to allocate values or jumps\n");
		ret = -1;
		goto finish;
	}
	memset(values, 0, source->dist_count*unique_arguments_count*sizeof(u64));
	memset(jumps,  0, unique_arguments_count*sizeof(int));
	memset(losses, 0, source->dist_count*sizeof(u64));
	/* follow all possible transitions */
	for (i = 0; i < source->dist_count; i++)
	{
		dist1 = source->dists[i];
		distN = source->dists[source->dist_count-1];
		p1 = dist1->packet_loss;
		p2 = distN->packet_loss;
		/* loss and loss */
		losses[i] += (p1*p2) >> 32;
		/* loss and delay */
		for (k = 0; k < distN->entries_count; k++)
		{
			val = distN->entries[k].x;
			/* find index of value in unique table */
			for (m = 0; m < unique_arguments_count; m++)
				if (unique_arguments[m] == val)
					break;
			if (m == unique_arguments_count)
			{
				print_info("bailout %d\n", __LINE__);
				ret = -1;
				goto finish;
			}
			jumps[m] = distN->entries[k].next_table;
			p4 = distN->entries[k].y_high - distN->entries[k].y_low + 1;
			values[i*unique_arguments_count+m] += (p1*p4) >> 32;
		}
		/* delay and ... */
		for (j = 0; j < dist1->entries_count; j++)
		{
			dist2 = source->dists[dist1->entries[j].next_table];
			/* delay and loss */
			p3 = dist1->entries[j].y_high - dist1->entries[j].y_low + 1;
			p4 = dist2->packet_loss;
			p5 = ((1ull<<32)-1-p1)*p3;
			p5 = ((p5 >> 32)*p4) >> 32;
			losses[i] += p5;
			/* delay and delay */
			for (k = 0; k < dist2->entries_count; k++)
			{
				val = dist2->entries[k].x;
				/* find index of value in unique table */
				for (m = 0; m < unique_arguments_count; m++)
					if (unique_arguments[m] == val)
						break;
				if (m == unique_arguments_count)
				{
					print_info("bailout %d\n", __LINE__);
					ret = -1;
					goto finish;
				}
				jumps[m] = dist2->entries[k].next_table;
				p4 = dist2->entries[k].y_high - dist2->entries[k].y_low + 1;
				p5 = ((1ull<<32)-1-p1)*p3;
				p5 = ((p5 >> 32)*p4) >> 32;
				values[i*unique_arguments_count+m] += p5;
			}
		}
	}
	/* prepare the interarrival structure */
	destination->interarrival = source->interarrival << 1;
	destination->dist_count = source->dist_count;
	destination->dists = kmalloc(destination->dist_count*sizeof(struct empath_dist *),
				     GFP_KERNEL);
	if (!destination->dists)
	{
		print_info("bailout %d\n",__LINE__);
		ret = -ENOMEM;
		goto finish;
	}
	for (i = 0; i < destination->dist_count; i++)
	{
		t = 0;
		for (j = 0; j < unique_arguments_count; j++)
			if (values[i*unique_arguments_count+j] > 0)
				t++;
		k = sizeof(struct empath_dist) + t*sizeof(struct empath_dist_entry);
		destination->dists[i] = kmalloc(k, GFP_KERNEL);
		if (! destination->dists[i])
		{
			print_info("line %d: unable to allocate dist %d with size %d\n",
				__LINE__, i, t);
			ret = -ENOMEM;
			goto finish;
		}
		dist = destination->dists[i];
		dist->entries_count = t;
		dist->packet_loss = losses[i];
		sum = 0;
		sum_low = 0;
		print_debug("distribution:\n");
		for (j = 0, k = 0; j < unique_arguments_count && k < t; j++)
		{
			if (values[i*unique_arguments_count+j] > 0)
			{
				dist->entries[k].x = unique_arguments[j];
				dist->entries[k].y_low = sum_low;
				sum += values[i*unique_arguments_count+j];
				print_debug("dist=%d x=%u y_low=%x y_high=%llx\n",
					i, dist->entries[k].x, dist->entries[k].y_low, sum);
				if (sum > 0xffffffff)
				{
					print_debug("line %d: probability %llx above 1.0\n",
						__LINE__, sum);
					ret = -1;
					goto finish;
				}
				dist->entries[k].y_high = sum;
				dist->entries[k].next_table = jumps[j];
				sum_low = sum + 1;
				k++;
			}
		}
		if (sum != 0xffffffff)
		{
			print_debug("line %d: fixing, probability below 1.0 > %llx\n",
				__LINE__, sum);
			/* calculate the limit for the weights */
			p1 = 0xffffffffull - sum;
			print_debug("line %d: surplus= %llx\n",
				__LINE__, p1);
			for (m = 31; m >= 0; m--)
			{
				p2 = 0;
				for (k = 0; k < dist->entries_count; k++)
				{
					p3 = dist->entries[k].y_high-dist->entries[k].y_low+1ull;
					p2 += p3 >> m;
				}
				if (p2 > p1)
					break;
			}
			print_debug("line %d: surplus_limit=%d\n",
				__LINE__, m);
			/* distribute the surplus from p1 */
			for (k = 0; k < dist->entries_count; k++)
			{
				p3 = dist->entries[k].y_high - dist->entries[k].y_low + 1ull;
				p3 >>= m;
				p3 = p3 > p1 ? p3 : p1;
				p1 -= p3;
				if (p3 <= 0)
					continue;
				dist->entries[k].y_high += p3;
				for (j = k+1; j < dist->entries_count; j++)
				{
					dist->entries[j].y_low += p3;
					dist->entries[j].y_high += p3;
				}
			}
			print_debug("distribution fixed:\n");
			for (k = 0; k < dist->entries_count; k++)
			{
				print_debug("dist=%d x=%u y_low=%x y_high=%x\n",
					i, dist->entries[k].x, 
					dist->entries[k].y_low, dist->entries[k].y_high);
			}
		}
	}
	/* all OK */
	ret = 0;
finish:
	kfree(unique_arguments);
	kfree(values);
	kfree(jumps);
	kfree(losses);
	return ret;
}

static void interarrival_prepare(struct empath_interarrival *inter)
{
	int i;
	print_debug("interarrival_prepare() start interarrival=%u distcount=%d\n", 
		inter->interarrival, inter->dist_count);
	/* create search trees for each distribution */
	for (i = 0; i < inter->dist_count; i++)
	{
		print_debug("interarrival_prepare() dist=%d\n", i);
		dist_prepare_search(inter->dists[i]);
	}
	print_debug("interarrival_prepare() end interarrival=%u\n", 
		inter->interarrival);
}



/* START empath scheduler */

/*
 * Insert one skb into qdisc.
 * Note: parent depends on return value to account for queue length.
 * 	NET_XMIT_DROP: queue length didn't change.
 *      NET_XMIT_SUCCESS: one skb was queued.
 */
static int empath_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	/* We don't fill cb now as skb_unshare() may invalidate it */
	struct empath_skb_cb *cb;
	psched_time_t now = psched_get_time();
	psched_tdiff_t delay;
	int ret, is_lost = 0;
	print_debug("enqueue skb=%p\n", skb);
	skb_orphan(skb);
	cb = empath_skb_cb(skb);
	/* calculate delay */
	delay = dist_generate(q, &is_lost, &now);
	if (is_lost)
	{
		sch->qstats.drops++;
		kfree_skb(skb);
		print_debug("enqueue drop\n");
		return NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
	}
	cb->time_to_send = now + delay;
	/* store it in queue */
	ret = qdisc_enqueue(skb, q->qdisc);
	if (likely(ret == NET_XMIT_SUCCESS)) {
		sch->q.qlen++;
		sch->bstats.bytes += qdisc_pkt_len(skb);
		sch->bstats.packets++;
	} else if (net_xmit_drop_count(ret)) {
		sch->qstats.drops++;
	}
	print_debug("enqueue ok\n");
	return ret;
}

/* Requeue packets but don't change time stamp */
static int empath_requeue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	int ret;

	if ((ret = q->qdisc->ops->requeue(skb, q->qdisc)) == 0) {
		sch->q.qlen++;
		sch->qstats.requeues++;
	}

	return ret;
}

static unsigned int empath_drop(struct Qdisc* sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	unsigned int len = 0;

	if (q->qdisc->ops->drop && (len = q->qdisc->ops->drop(q->qdisc)) != 0) {
		sch->q.qlen--;
		sch->qstats.drops++;
	}
	return len;
}

static struct sk_buff *empath_dequeue(struct Qdisc *sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb;


	smp_mb();
	if (sch->flags & TCQ_F_THROTTLED)
		return NULL;

	skb = q->qdisc->dequeue(q->qdisc);	
	if (skb) {
		const struct empath_skb_cb *cb = empath_skb_cb(skb);
		psched_time_t now = psched_get_time();
		/* is it time to send? */
		if (cb->time_to_send <= now)
		{
			print_debug("dequeue skb=%p\n", skb);
			sch->q.qlen--;
			return skb;
		}
		if (unlikely(q->qdisc->ops->requeue(skb, q->qdisc) != NET_XMIT_SUCCESS)) {
			qdisc_tree_decrease_qlen(q->qdisc, 1);
			sch->qstats.drops++;
			print_info("%s could not requeue\n",
			       q->qdisc->ops->id);
		}
		qdisc_watchdog_schedule(&q->watchdog, cb->time_to_send);
	}

	return NULL;
}

static void empath_reset(struct Qdisc *sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);

	print_debug("reset\n");

	qdisc_reset(q->qdisc);
	sch->q.qlen = 0;
	qdisc_watchdog_cancel(&q->watchdog);
}

/*
 * Sets all parameters of the emulator.
 */
static int get_parameters(struct Qdisc *sch, const struct nlattr *attr)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	unsigned int total_length = nla_len(attr);
	unsigned int used_length = 0;
	struct tc_empath_parameters *params = nla_data(attr);
	struct tc_empath_dist *dist;
	struct empath_interarrival *interarrivals, *old_interarrivals;
	struct empath_dist **dist_table;
	spinlock_t *root_lock;
	int i, j;
	int interarrival_count, old_interarrival_count;
	int ret;
	u32 t1;
	print_debug("NLA length %d\n", total_length);
	/* check main params struct */
	used_length += sizeof(struct tc_empath_parameters);
	if (used_length > total_length)
	{
		print_info("invalid NLA message length\n");
		return -EINVAL;
	}
	if (params->interarrival > params->interarrival_scope )
	{
		params->interarrival = params->interarrival_scope;
	}
	/* calculate number of interarrivals entries */
	i = 0;
	t1 = params->interarrival;
	while (t1 <= params->interarrival_scope && t1 <= 0x0fffffff)
	{
		/*print_debug("increasing %d %d %d\n",
		  t1, params->interarrival_scope, t1 < params->interarrival_scope);*/
		t1 <<= 1;
		i++;
	}
	interarrival_count = i;
	/* allocate interarrivals table */
	interarrivals = kmalloc(interarrival_count*sizeof(struct empath_interarrival),
				   GFP_KERNEL);
	if (! interarrivals)
	{
		print_info("unable to allocate interarrivals\n");
		ret = -ENOMEM;
		goto error;
	}
	memset(interarrivals, 0, 
	       interarrival_count*sizeof(struct empath_interarrival));
	t1 = params->interarrival;
	for (i = 0; i < interarrival_count; i++)
	{
		interarrivals[i].interarrival = t1;
		print_debug("initializing interarrival %d\n", t1);
		t1 <<= 1;
	}

	if (params->dist_count < 2)
	{
		print_info("need at least 2 distribution tables\n");
		ret = -EINVAL;
		goto error;
	}

	/* allocate intearravials structure for base interarrival */
	interarrivals[0].interarrival = params->interarrival;
	interarrivals[0].dist_count = params->dist_count;

	/* allocate dist table */
	dist_table = kmalloc(params->dist_count*sizeof(struct empath_dist *),
			     GFP_KERNEL);
	if (! dist_table)
	{
		print_info("unable to allocate base dist_table\n");
		ret = -ENOMEM;
		goto error;
	}
	memset(dist_table, 0, params->dist_count*sizeof(struct empath_dist *));
	/* copy each dist table for base interarrival */
	print_debug("dist_count=%d\n", params->dist_count);
	dist = & params->dists[0];
	for (i = 0; i < params->dist_count; i++)
	{
		int t;
		void *a1, *a2;
		u64 sum;
		/* check message size */
		used_length += sizeof(struct tc_empath_dist);
		if (used_length > total_length)
		{
			print_info("invalid NLA message length\n");
			ret = -EINVAL;
			goto error;
		}
		print_debug("dist %d dist_count=%d loss=%x\n", 
			i, dist->entries_count, dist->packet_loss);
		t = sizeof(struct empath_dist) 
			+ dist->entries_count*sizeof(struct empath_dist_entry);
		dist_table[i] =	kmalloc(t, GFP_KERNEL);
		if (! dist_table[i])
		{
			print_info("unable to allocate dist %d with size %d\n",
				i, t);
			ret = -ENOMEM;
			goto error;
		}
		memset(dist_table[i], 0, t);
		used_length += dist->entries_count*sizeof(struct tc_empath_dist_entry);
		if (used_length > total_length)
		{
			print_info("invalid NLA message length\n");
			ret = -EINVAL;
			goto error;
		}
		
		if (dist->entries_count == 0)
		{
			print_info("need at least 1 point in distribution\n");
			ret = -EINVAL;
			goto error;
		}

		/* copy data */
		dist_table[i]->entries_count = dist->entries_count;
		dist_table[i]->packet_loss = dist->packet_loss;
		sum = 0;
		for (j = 0; j < dist->entries_count; j++)
		{
			struct empath_dist_entry *e = & dist_table[i]->entries[j];
			e->x = dist->entries[j].x;
			e->y_low = sum;
			e->y_high = dist->entries[j].y_high;
			e->next_table = dist->entries[j].next_table;
			if (e->next_table < 0 || e->next_table >= params->dist_count)
			{
				print_info("next_table=%d while there are %d tables\n",
					   e->next_table, params->dist_count);
				ret = -EINVAL;
				goto error;
			}
			print_debug("entry %d x=%d y_low=%x y_high=%x next=%d\n", 
				i, e->x, e->y_low, e->y_high, e->next_table);
			sum = (u64) e->y_high + 1;
		}
		if (sum != 0x0100000000ull)
		{
			print_info("line %d: fixing delay distribution %llu %u\n",
				__LINE__, sum,
				(unsigned int)dist_table[i]->entries[dist->entries_count-1].y_high);
			dist_table[i]->entries[dist->entries_count-1].y_high = 0xffffffff;
		}
		a1 = dist;
		t = sizeof(struct tc_empath_dist) 
			+ dist->entries_count*sizeof(struct tc_empath_dist_entry);
		/*print_info("moving dist by %d bytes\n", t);
		 */
		dist = (struct tc_empath_dist *) ((char *) dist + t);
		a2 = dist;
		/*print_info("dist moved by %d\n", a2-a1);
		 */
	}
	/* clean up if anything went wrong */
	if (used_length != total_length)
	{
		print_info("there are remaining elements in the message...\n");
		ret = -EINVAL;
		goto error;
	}
	interarrivals[0].dists = dist_table;
	interarrival_prepare(&interarrivals[0]);
	/* fill in other interarrivals */
	for (i = 1; i < interarrival_count; i++)
	{
		if (interarrival_power(&interarrivals[i-1],
				      &interarrivals[i]) < 0)
		{
			print_info("failed to square the interarrival %u\n", 
				interarrivals[i-1].interarrival << 1);
			goto error;
		}
		interarrival_prepare(&interarrivals[i]);
	}
	/* commit phase starts */
	/* change queue size */
	ret = fifo_set_limit(q->qdisc, params->queue_limit);
	if (ret) {
		print_info("can't set fifo limit\n");
		goto error;
	}
	/* swap states */
	root_lock = qdisc_root_sleeping_lock(sch);
	spin_lock_bh(root_lock);

	old_interarrival_count = q->interarrival_count;
	old_interarrivals = q->interarrivals;
	q->interarrival_count= interarrival_count;
	q->dist_state = params->dist_count-1;
	q->dist_count = params->dist_count;
	q->interarrivals = interarrivals;

	spin_unlock_bh(root_lock);
	/* commit phase ends */
	/* release old structures */
	for (i = 0; i < old_interarrival_count; i++)
	{
		for (j = 0; j < old_interarrivals[i].dist_count; j++)
		{
			kfree(old_interarrivals[i].dists[j]);
		}
		kfree(old_interarrivals[i].dists);
	}
	kfree(old_interarrivals);
	return 0;
error:
	/* clean the structures */
	for (i = 0; i < interarrival_count && interarrivals; i++)
	{
		for (j = 0; j < interarrivals[i].dist_count && interarrivals[i].dists; j++)
		{
			kfree(interarrivals[i].dists[j]);
		}
		kfree(interarrivals[i].dists);
	}
	kfree(interarrivals);

	return ret;
}


static const struct nla_policy empath_policy[TCA_EMPATH_MAX + 1] = {
};

/* Parse netlink message to set options */
static int empath_change(struct Qdisc *sch, struct nlattr *opt)
{
	/*struct empath_sched_data *q = qdisc_priv(sch);*/
	struct nlattr *tb[TCA_EMPATH_MAX + 1];
	int ret;

	print_debug("empath_change\n");

	if (opt == NULL)
		return -EINVAL;

	ret = nla_parse_nested(tb, TCA_EMPATH_MAX, opt, empath_policy);
	if (ret < 0)
		return ret;

	if (tb[TCA_EMPATH_PARAMETERS]) {
		ret = get_parameters(sch, tb[TCA_EMPATH_PARAMETERS]);
		if (ret)
			return ret;
	} else {
		return -EINVAL;
	}

	return 0;
}


static int empath_init(struct Qdisc *sch, struct nlattr *opt)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	int ret;
	
	print_debug("empath_init\n");

	if (!opt)
		return -EINVAL;

	qdisc_watchdog_init(&q->watchdog, sch);

	q->qdisc = qdisc_create_dflt(qdisc_dev(sch), sch->dev_queue,
				     &pfifo_qdisc_ops,
				     TC_H_MAKE(sch->handle, 1));
	if (!q->qdisc) {
		print_info("qdisc create failed\n");
		return -ENOMEM;
	}
	q->interarrivals = NULL;
	q->interarrival_count = 0;
	q->dist_state = 0;
	q->arrival = PSCHED_PASTPERFECT;
	q->previous_delay = 0;
	q->scope_delay = 5000000; /* 5 seconds scope */
	ret = empath_change(sch, opt);
	if (ret) {
		print_info("change failed\n");
		qdisc_destroy(q->qdisc);
	}
	return ret;
}

static void empath_destroy(struct Qdisc *sch)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	int i, j;

	print_debug("empath_destroy\n");

	qdisc_watchdog_cancel(&q->watchdog);
	qdisc_destroy(q->qdisc);
	for (i = 0; i < q->interarrival_count; i++)
	{
		for (j = 0; j < q->interarrivals[i].dist_count; j++)
		{
			kfree(q->interarrivals[i].dists[j]);
		}
		kfree(q->interarrivals[i].dists);
	}
	kfree(q->interarrivals);
}

static int empath_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	print_debug("empath_dump\n");
	/* TODO: add reporting */
	/*
	const struct empath_sched_data *q = qdisc_priv(sch);
	unsigned char *b = skb_tail_pointer(skb);
	struct nlattr *nla = (struct nlattr *) b;
	struct tc_netem_qopt qopt;
	struct tc_netem_corr cor;
	struct tc_netem_reorder reorder;
	struct tc_netem_corrupt corrupt;

	qopt.latency = q->latency;
	qopt.jitter = q->jitter;
	qopt.limit = q->limit;
	qopt.loss = q->loss;
	qopt.gap = q->gap;
	qopt.duplicate = q->duplicate;
	NLA_PUT(skb, TCA_OPTIONS, sizeof(qopt), &qopt);

	cor.delay_corr = q->delay_cor.rho;
	cor.loss_corr = q->loss_cor.rho;
	cor.dup_corr = q->dup_cor.rho;
	NLA_PUT(skb, TCA_NETEM_CORR, sizeof(cor), &cor);

	reorder.probability = q->reorder;
	reorder.correlation = q->reorder_cor.rho;
	NLA_PUT(skb, TCA_NETEM_REORDER, sizeof(reorder), &reorder);

	corrupt.probability = q->corrupt;
	corrupt.correlation = q->corrupt_cor.rho;
	NLA_PUT(skb, TCA_NETEM_CORRUPT, sizeof(corrupt), &corrupt);

	nla->nla_len = skb_tail_pointer(skb) - b;
	*/
	return skb->len;
}

/* FINISH empath scheduler */

/* START empath class */

static int empath_dump_class(struct Qdisc *sch, unsigned long cl,
			  struct sk_buff *skb, struct tcmsg *tcm)
{
	struct empath_sched_data *q = qdisc_priv(sch);

	if (cl != 1) 	/* only one class */
		return -ENOENT;

	tcm->tcm_handle |= TC_H_MIN(1);
	tcm->tcm_info = q->qdisc->handle;

	return 0;
}

static int empath_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
		     struct Qdisc **old)
{
	struct empath_sched_data *q = qdisc_priv(sch);

	if (new == NULL)
		new = &noop_qdisc;

	sch_tree_lock(sch);
	*old = xchg(&q->qdisc, new);
	qdisc_tree_decrease_qlen(*old, (*old)->q.qlen);
	qdisc_reset(*old);
	sch_tree_unlock(sch);

	return 0;
}

static struct Qdisc *empath_leaf(struct Qdisc *sch, unsigned long arg)
{
	struct empath_sched_data *q = qdisc_priv(sch);
	return q->qdisc;
}

static unsigned long empath_get(struct Qdisc *sch, u32 classid)
{
	return 1;
}

static void empath_put(struct Qdisc *sch, unsigned long arg)
{
}

static int empath_change_class(struct Qdisc *sch, u32 classid, u32 parentid,
			    struct nlattr **tca, unsigned long *arg)
{
	return -ENOSYS;
}

static int empath_delete(struct Qdisc *sch, unsigned long arg)
{
	return -ENOSYS;
}

static void empath_walk(struct Qdisc *sch, struct qdisc_walker *walker)
{
	if (!walker->stop) {
		if (walker->count >= walker->skip)
			if (walker->fn(sch, 1, walker) < 0) {
				walker->stop = 1;
				return;
			}
		walker->count++;
	}
}

static struct tcf_proto **empath_find_tcf(struct Qdisc *sch, unsigned long cl)
{
	return NULL;
}

static const struct Qdisc_class_ops empath_class_ops = {
	.graft		=	empath_graft,
	.leaf		=	empath_leaf,
	.get		=	empath_get,
	.put		=	empath_put,
	.change		=	empath_change_class,
	.delete		=	empath_delete,
	.walk		=	empath_walk,
	.tcf_chain	=	empath_find_tcf,
	.dump		=	empath_dump_class,
};

static struct Qdisc_ops empath_qdisc_ops __read_mostly = {
	.id		=	"empath",
	.cl_ops		=	&empath_class_ops,
	.priv_size	=	sizeof(struct empath_sched_data),
	.enqueue	=	empath_enqueue,
	.dequeue	=	empath_dequeue,
	.requeue	=	empath_requeue,
	.drop		=	empath_drop,
	.init		=	empath_init,
	.reset		=	empath_reset,
	.destroy	=	empath_destroy,
	.change		=	empath_change,
	.dump		=	empath_dump,
	.owner		=	THIS_MODULE,
};


static int __init empath_module_init(void)
{
	int ret;
	ret = register_qdisc(&empath_qdisc_ops);
	if (ret==0)
		print_info("load version " VERSION "\n");
	return ret;
}
static void __exit empath_module_exit(void)
{
	int ret;
	ret = unregister_qdisc(&empath_qdisc_ops);
	print_info("unload\n");
}
module_init(empath_module_init)
module_exit(empath_module_exit)
MODULE_LICENSE("GPL");
